﻿namespace Arms.Framework.Math
{
    using Arms.Framework.Objects.Geometry;
    using System;

    public static class Spatial
    {
        /// <summary>
        /// Creates a square with the size of the supplied distance radius. 
        /// Accounts for curvature of the earth.
        /// </summary>
        /// <param name="longitude">Longitude(x) coordinate of the center</param>
        /// <param name="latitude">Latitude(y) coordinate of the center</param>
        /// <param name="distanceRadius">Distance from the center to the corner</param>
        /// <returns></returns>
        public static Square CreateSquareFromPoint(double longitude, double latitude, double distanceRadius)
        {
            return Spatial.CreateSquareFromPoint(new Coordinate(longitude, latitude), distanceRadius);
        }
        /// <summary>
        /// Creates a square with the size of the supplied distance radius. 
        /// Accounts for curvature of the earth.
        /// </summary>
        /// <param name="origin">Coordinate of the center</param>
        /// <param name="distanceRadius">Distance from the center to the corner</param>
        /// <returns></returns>
        public static Square CreateSquareFromPoint(Coordinate origin, double distanceRadius)
        {
            double distanceDivideByRadius = distanceRadius / Constants.EarthRadius;

            double upperLeftLatitude = origin.Y + distanceDivideByRadius.ToDegrees();
            double upperLeftLongitude = origin.X - (distanceDivideByRadius / Math.Cos(origin.X.ToRadians())).ToDegrees();
            double lowerRightLatitude = origin.Y - distanceDivideByRadius.ToDegrees();
            double lowerRightLongitude = origin.X + (distanceDivideByRadius / Math.Cos(origin.X.ToRadians())).ToDegrees();

            return new Square(upperLeftLongitude, upperLeftLatitude, lowerRightLongitude, lowerRightLatitude);
        }
        /// <summary>
        /// Creates a square with a size equal to the distance between origin and destination
        /// </summary>
        /// <param name="latitudeA"></param>
        /// <param name="longitudeA"></param>
        /// <param name="latitudeB"></param>
        /// <param name="LongitudeB"></param>
        /// <returns></returns>
        public static Square CreateSquareFromTwoPoints(double latitudeA, double longitudeA, double latitudeB, double LongitudeB)
        {
            return Spatial.CreateSquareFromTwoPoints(new Coordinate(longitudeA, latitudeA), new Coordinate(LongitudeB, latitudeB));
        }
        /// <summary>
        /// Creates a square with a size equal to the distance between origin and destination
        /// </summary>
        /// <param name="origin"></param>
        /// <param name="destination"></param>
        /// <returns></returns>
        public static Square CreateSquareFromTwoPoints(Coordinate origin, Coordinate destination)
        {
            var midPoint = origin.MidpointTo(destination);
            var distanceRadius = midPoint.DistanceTo(origin);

            return Spatial.CreateSquareFromPoint(midPoint, distanceRadius);
        }
        /// <summary>
        /// Calculates the distance between 2 coordinates.
        /// http://en.wikipedia.org/wiki/Haversine_formula
        /// http://www.movable-type.co.uk/scripts/latlong.html
        /// </summary>
        /// <remarks>
        /// d = acos( sin(φ1).sin(φ2) + cos(φ1).cos(φ2).cos(Δλ) ).R
        /// </remarks>
        /// <param name="origin"></param>
        /// <param name="destination"></param>
        /// <returns></returns>
        public static double DistanceTo(this Coordinate origin, Coordinate destination)
        {
            double latDistance = (destination.Y - origin.Y).ToRadians();
            double longDistance = (destination.X - origin.X).ToRadians();
            double latOrigin = origin.Y.ToRadians();
            double latDestination = origin.Y.ToRadians();

            var a = Math.Sin(latDistance / 2) *
                    Math.Sin(latDistance / 2) +
                    Math.Sin(longDistance / 2) *
                    Math.Sin(longDistance / 2) *
                    Math.Cos(latOrigin) *
                    Math.Cos(latDestination);

            var c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));

            return Constants.EarthRadius * c;
        }
        /// <summary>
        /// This is the half-way point along a great circle path between the two given coordinates.
        /// http://en.wikipedia.org/wiki/Haversine_formula
        /// http://www.movable-type.co.uk/scripts/latlong.html
        /// </summary>
        /// <remarks>
        /// Bx = cos(φ2).cos(Δλ)
        /// By = cos(φ2).sin(Δλ)
        /// φm = atan2( sin(φ1) + sin(φ2), √((cos(φ1)+Bx)² + By²) )
        /// λm = λ1 + atan2(By, cos(φ1)+Bx)
        /// </remarks>
        /// <param name="origin"></param>
        /// <param name="destination"></param>
        /// <returns></returns>
        public static Coordinate MidpointTo(this Coordinate origin, Coordinate destination)
        {
            double latDistance = (destination.Y - origin.Y).ToRadians();
            double longDistance = (destination.X - origin.X).ToRadians();
            double latOrigin = origin.Y.ToRadians();
            double longOrigin = origin.X.ToRadians();
            double latDestination = origin.Y.ToRadians();

            var bx = Math.Cos(latDestination) * Math.Cos(longDistance);
            var by = Math.Cos(latDestination) * Math.Sin(longDistance);
            var latFinal = Math.Atan2(Math.Sin(latOrigin) + Math.Sin(latDestination), Math.Sqrt((Math.Cos(latOrigin) + bx) * (Math.Cos(latOrigin) + bx) + by * by));
            var longFinal = longOrigin + Math.Atan2(by, Math.Cos(latOrigin) + bx);

            return new Coordinate(longFinal.ToDegrees(), latFinal.ToDegrees());
        }
        /// <summary>
        /// This formula is for the initial bearing (sometimes referred to as forward azimuth) 
        /// which if followed in a straight line along a great-circle arc 
        /// will take you from the start point to the end point.
        /// </summary>
        /// <remarks>
        /// θ = atan2( sin(Δλ).cos(φ2), cos(φ1).sin(φ2) − sin(φ1).cos(φ2).cos(Δλ) )
        /// </remarks>
        /// <param name="origin"></param>
        /// <param name="destination"></param>
        /// <returns></returns>
        public static double BearingTo(this Coordinate origin, Coordinate destination)
        {
            double latDistance = (destination.Y - origin.Y).ToRadians();
            double longDistance = (destination.X - origin.X).ToRadians();
            double latOrigin = origin.Y.ToRadians();
            double latDestination = origin.Y.ToRadians();

            var y = Math.Sin(longDistance) * Math.Cos(latDestination);
            var x = (Math.Sin(latOrigin) * Math.Sin(latDestination)) -
                    (Math.Sin(latOrigin) * Math.Cos(latDestination) * Math.Cos(longDistance));

            return Math.Atan2(y, x).ToDegrees();
        }
    }
}
